
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_arming.c
  * @author     baiyang
  * @date       2021-8-18
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_arming.h"
#include <copter/fms.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
bool arming_rc_arm_checks(gp_arming* arming, ArmingMethod method);
bool arming_manual_transmitter_checks(gp_arming * arming, bool report);

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       构造函数
  * @param[in]   arming  
  * @param[out]  
  * @retval      
  * @note        
  */
void arming_ctor(gp_arming* arming)
{
    // method that was last used for disarm; invalid unless the
    // vehicle has been disarmed at least once.
    arming->_last_disarm_method = ARMING_CHECK_UNKNOWN;
}

// mandatory checks that cannot be bypassed.  This function will only be called if ARMING_CHECK is zero or arming forced
bool arming_mandatory_checks(gp_arming* arming, bool report) { return true; }

bool arming_is_armed(gp_arming* arming)
{
    return (ArmingRequired)arming->require == ARMING_NO || arming->armed;
}

//returns true if arming occurred successfully
bool arming_arm(gp_arming* arming, ArmingMethod method, const bool do_arming_checks)
{
    if (arming->armed) { //already armed
        return false;
    }

    if ((!do_arming_checks && arming_mandatory_checks(arming, true)) || (arming->ops->pre_arm_checks(arming, true) && arming->ops->arm_checks(arming, method))) {
        arming->armed = true;

        //Log_Write_Arm(!do_arming_checks, method); // note Log_Write_Armed takes forced not do_arming_checks

    } else {
        //AP::logger().arming_failure();
        arming->armed = false;
    }

    return arming->armed;
}

//returns true if disarming occurred successfully
bool arming_disarm(gp_arming* arming, ArmingMethod method, bool do_disarm_checks)
{
    if (!arming->armed) { // already disarmed
        return false;
    }
    arming->armed = false;
    arming->_last_disarm_method = method;

    //Log_Write_Disarm(method); // should be able to pass through force here?

    return true;
}

bool arming_pre_arm_checks(gp_arming* arming, bool report)
{
    if (arming->armed || arming->require == (uint8_t)ARMING_NO) {
        // if we are already armed or don't need any arming checks
        // then skip the checks
        return true;
    }

#if 0
    return hardware_safety_check(report)
        &  barometer_checks(report)
        &  ins_checks(report)
        &  compass_checks(report)
        &  gps_checks(report)
        &  battery_checks(report)
        &  logging_checks(report)
        &  manual_transmitter_checks(report)
        &  mission_checks(report)
        &  rangefinder_checks(report)
        &  servo_checks(report)
        &  board_voltage_checks(report)
        &  system_checks(report)
        &  can_checks(report)
        &  generator_checks(report)
        &  proximity_checks(report)
        &  camera_checks(report)
        &  osd_checks(report)
        &  fettec_checks(report)
        &  visodom_checks(report)
        &  aux_auth_checks(report)
        &  disarm_switch_checks(report)
        &  fence_checks(report);
#endif
    return arming_manual_transmitter_checks(arming, report);
}

bool arming_arm_checks(gp_arming* arming, ArmingMethod method)
{
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_RC)) {
        if (!arming_rc_arm_checks(arming, method)) {
            return false;
        }
    }

    // note that this will prepare AP_Logger to start logging
    // so should be the last check to be done before arming

    // Note also that we need to PrepForArming() regardless of whether
    // the arming check flag is set - disabling the arming check
    // should not stop logging from working.
#if 0
    AP_Logger *logger = AP_Logger::get_singleton();
    if (logger->logging_present()) {
        // If we're configured to log, prep it
        logger->PrepForArming();
        if (!logger->logging_started() &&
            ((checks_to_perform & ARMING_CHECK_ALL) ||
             (checks_to_perform & ARMING_CHECK_LOGGING))) {
            check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
            return false;
        }
    }
#endif

    return true;
}

bool arming_check_enabled(gp_arming * arming, const ArmingChecks check)
{
    if (arming->checks_to_perform & ARMING_CHECK_ALL) {
        return true;
    }
    return (arming->checks_to_perform & check);
}

MAV_SEVERITY arming_check_severity(gp_arming * arming, const ArmingChecks check)
{
    if (arming_check_enabled(arming, check)) {
        return MAV_SEVERITY_CRITICAL;
    }
    return MAV_SEVERITY_DEBUG; // technically should be NOTICE, but will annoy users at that level
}

void arming_check_failed(gp_arming * arming, bool report, const char *fmt, ...)
{
    if (!report) {
        return;
    }

    char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
    snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
    va_list arg_list;
    va_start(arg_list, fmt);
    mavproxy_send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
    va_end(arg_list);
}

void arming_check_failed2(gp_arming * arming, const ArmingChecks  check, bool report, const char *fmt, ...)
{
    if (!report) {
        return;
    }
    char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
    snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
    MAV_SEVERITY severity = arming_check_severity(arming, check);
    va_list arg_list;
    va_start(arg_list, fmt);
    mavproxy_send_textv(severity, taggedfmt, arg_list);
    va_end(arg_list);
}

bool arming_rc_arm_checks(gp_arming* arming, ArmingMethod method)
{
    // don't check the trims if we are in a failsafe
    if (!RCs_has_valid_input()) {
        return true;
    }

    // only check if we've received some form of input within the last second
    // this is a protection against a vehicle having never enabled an input
    uint32_t last_input_ms = fms.channels.last_update_ms;
    if ((last_input_ms == 0) || ((time_millis() - last_input_ms) > 1000)) {
        return true;
    }

    bool check_passed = true;
    // ensure all rc channels have different functions
    if (RCs_duplicate_options_exist(&fms.channels)) {
        arming_check_failed2(arming, ARMING_CHECK_PARAMETERS, true, "Duplicate Aux Switch Options");
        check_passed = false;
    }
    if (RCs_flight_mode_channel_conflicts_with_rc_option(&fms.channels)) {
        arming_check_failed2(arming, ARMING_CHECK_PARAMETERS, true, "Mode channel and RC%d_OPTION conflict", RCs_flight_mode_channel_number());
        check_passed = false;
    }
    
    return check_passed;
}

bool arming_rc_calibration_checks(gp_arming * arming, bool report)
{
    bool check_passed = true;
    const uint8_t num_channels = RCs_get_valid_channel_count();
    for (uint8_t i = 0; i < MAX_RC_CHANNEL_NUM; i++) {
        RC_HandleTypeDef *c = &fms.channels.channel[i];
        if (c == NULL) {
            continue;
        }
        if (i >= num_channels && !RC_has_override(c)) {
            continue;
        }
        const uint16_t trim = c->_radio_trim;
        if (c->_radio_min > trim) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "RC%d_MIN is greater than RC%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
        if (c->_radio_max < trim) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "RC%d_MAX is less than RC%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
    }

    return check_passed;
}

// Copter and sub share the same RC input limits
// Copter checks that min and max have been configured by default, Sub does not
bool arming_rc_checks_copter_sub(gp_arming * arming, const bool display_failure)
{
    const RC_HandleTypeDef *channels[] = {
        fms.channel_roll,
        fms.channel_pitch,
        fms.channel_throttle,
        fms.channel_yaw
    };
        
    // set rc-checks to success if RC checks are disabled
    if ((arming->checks_to_perform != ARMING_CHECK_ALL) && !(arming->checks_to_perform & ARMING_CHECK_RC)) {
        return true;
    }

    bool ret = true;

    const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };

    for (uint8_t i=0; i<ARRAY_SIZE(channel_names);i++) {
        const RC_HandleTypeDef *channel = channels[i];
        const char *channel_name = channel_names[i];
        // check if radio has been calibrated
        if (channel->_radio_min > RC_CALIB_MIN_LIMIT_PWM) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio min too high", channel_name);
            ret = false;
        }
        if (channel->_radio_max < RC_CALIB_MAX_LIMIT_PWM) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio max too low", channel_name);
            ret = false;
        }
        bool fail = true;
        if (i == 2) {
            // skip checking trim for throttle as older code did not check it
            fail = false;
        }
        if (channel->_radio_trim < channel->_radio_min) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio trim below min", channel_name);
            if (fail) {
                ret = false;
            }
        }
        if (channel->_radio_trim > channel->_radio_max) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio trim above max", channel_name);
            if (fail) {
                ret = false;
            }
        }
    }
    return ret;
}

bool arming_manual_transmitter_checks(gp_arming * arming, bool report)
{
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_RC)) {

        if (fms.failsafe.radio) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "Radio failsafe on");
            return false;
        }

        if (!arming->ops->rc_calibration_checks(arming, report)) {
            return false;
        }
    }

    return true;
}

/*------------------------------------test------------------------------------*/


